/*
This file is part of MMM.

MMM is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.

MMM is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
GNU General Public License for more details.

You should have received a copy of the GNU General Public License
along with MMM.  If not, see <http://www.gnu.org/licenses/>.
*
* @package    MMM
* @author     Andre Meixner
* @copyright  2018 High Performance Humanoid Technologies (H2T), Karlsruhe, Germany
*
*/

#include "MMMModelConverterConfiguration.h"
#include <MMM/Motion/MotionWriterXML.h>
#include <MMM/Model/ModelReaderXML.h>
#include <MMM/Motion/Plugin/ModelPosePlugin/ModelPoseSensor.h>

using namespace MMM;

int main(int argc, char *argv[]) {
    MMM_INFO << " --- MMMModelConverter --- " << endl;
    MMMModelConverterConfiguration* configuration = new MMMModelConverterConfiguration();
    if (!configuration->processCommandLine(argc, argv))
    {
        MMM_ERROR << "Error while processing command line, aborting..." << endl;
        return -1;
    }

    try {
        MMM_INFO << "Reading model file..." << std::endl;
        MMM::ModelReaderXMLPtr reader(new MMM::ModelReaderXML());
        MMM::ModelPtr model = reader->loadModel(configuration->inputModelPath);

        MMM::MotionPtr motion(new MMM::Motion(configuration->motionName, model, model, nullptr, configuration->inputModelPath));

        MMM::ModelPoseSensorPtr modelPoseSensor(new ModelPoseSensor());
        MMM::ModelPoseSensorMeasurementPtr modelPoseSensorMeasurement(new MMM::ModelPoseSensorMeasurement(0.0f, Eigen::Vector3f(0,0,0), Eigen::Vector3f(0,0,0)));
        modelPoseSensor->addSensorMeasurement(modelPoseSensorMeasurement);
        motion->addSensor(modelPoseSensor);

        MMM::MotionWriterXML::writeMotion(motion, configuration->outputMotionPath);
        return 0;
    } catch (MMM::Exception::MMMException &e) {
        MMM_ERROR << e.what() << endl;
        return -2;
    }
}
